#include "Fifo.h"
#include <string.h>

#undef min
#define min(a, b) ((a) < (b) ? (a) : (b))

static void __fifo_in(struct fifo *fifo, const uint8_t *src, 
	unsigned int len,unsigned int off)
{
	unsigned int l;
	off &= fifo->mask;
	l = min(len,(fifo->mask + 1) - off);
	
	memcpy(fifo->data + off,src,l);
	memcpy(fifo->data, src + l, len - l);
}

static void __fifo_out(struct fifo *fifo, uint8_t *des,
			unsigned int len, unsigned int off)
{
	unsigned int l;

	off &= fifo->mask;
	l = min(len, (fifo->mask + 1) - off);

	memcpy(des, fifo->data + off, l);
	memcpy(des + l, fifo->data, len - l);
}

unsigned int fifo_in_for_dma(struct fifo *fifo, const uint8_t *src, unsigned int len)
{
	unsigned int l;

	l = fifo_space(fifo);
	if (len > l)
		len = l;

	fifo->in += len;
	return len;
}

unsigned int fifo_in(struct fifo *fifo, const uint8_t *src, unsigned int len)
{
	unsigned int l;

	l = fifo_space(fifo);
	if (len > l)
		len = l;

	__fifo_in(fifo,src,len,fifo->in);
	
	fifo->in += len;
	return len;
}

unsigned int fifo_out(struct fifo *fifo, uint8_t *des, unsigned int len)
{
	unsigned int l;

	l = fifo->in - fifo->out;
	if (len > l)
		len = l;

	__fifo_out(fifo, des, len, fifo->out);
	fifo->out += len;
	return len;
}

struct fifo *fifo_alloc(unsigned int size)
{
	struct fifo *fifo;

	fifo = pvPortMalloc(size + sizeof(struct fifo));
	if (!fifo)
		return NULL;
	fifo->data = (uint8_t *)fifo + sizeof(struct fifo);
	fifo->in = fifo->out = 0;
	fifo->mask = size - 1;

	return fifo;
}

void fifo_free(struct fifo *fifo)
{
	vPortFree(fifo);
}

void fifo_init(struct fifo *fifo, uint8_t *data, unsigned int size)
{
	fifo->data = data;
	fifo->mask = size - 1;
	fifo->in = fifo->out = 0;
}
